/*
 * @Author: greyqiu greyqiu@163.com
 * @Date: 2023-07-07 09:59:18
 * @LastEditors: greyqiu greyqiu@163.com
 * @LastEditTime: 2023-07-07 11:15:18
 * @FilePath: \MDK-ARMe:\开发进行中\Optimizer-NA32G43x_1\common\easy_pid_f32.c
 * @Description: 
 * 
 * Copyright (c) 2023 by GreyQiu, All Rights Reserved. 
 */

#include <stddef.h>
#include <stdbool.h>
#include <stdint.h>
#include "easy_pid_f32.h"


/**
 * @description: 初始化PID控制器
 * @param {arm_pid_instance_f32} *S 
 * @return {*}
 */
void easy_pid_config(easy_pid_f32_t *S, uint32_t isrFreq, float Kp, float Ki, float Kd)
{
    S->A0 = Kp + Ki / isrFreq + Kd * isrFreq;
    S->A1 = -(Kp + Kd * isrFreq * 2);
    S->A2 = Kd;
    S->Xn_2 = 0;
    S->Xn_1 = 0;
    S->Yn_1 = 0;
    S->ref = 0;
}
